#ifndef NAVIGATION_H_
#define NAVIGATION_H_

#include "Gps.h"

namespace uav
{
/**
 * @namespace uav::nav
 * @brief The namespace for the navigation system.
 *
 * The navigation system is responsible for tracking the position and orientation of the UAV.
 */
namespace nav
{

using namespace math;

const Fixed NAV_G = Fixed(9.80665); ///< Gravitational acceleration constant in @f$ m / s^2 @f$.

/**
 * @class Navigation
 *
 * @brief %Navigation system class responsible for tracking position orientation based on IMU
 * and GPS data.
 *
 * @author Travis Portz
 */
class Navigation
{
public:
	/**
	 * Constructs a navigation system instance.
	 *
	 * @param imuOffset IMU offset from the UAV's center-of-gravity in meters.
	 * @param gpsOffset GPS antenna offset from the UAV's center-of-gravity in meters.
	 */
	Navigation(const Vector3 &imuOffset, const Vector3 &gpsOffset);

	/**
	 * Initializes the navigation system.
	 *
	 * This should function should be called after calibration of the IMU and GPS.
	 *
	 * This function will set the gyro and accelerometer bias estimates to zero
	 * assuming that the zeros of the IMU are determined during calibration and
	 * are subsequently used for determining the inputs to the update() function.
	 *
	 * @param ref The ENU reference frame to use as the navigation frame.
	 * @param p   The initial position in the navigation frame.
	 * @param v   The initial velocity in the navigation frame.
	 * @param q   The initial orientation quaternion (navigation to body).
	 */
	void initialize(const RefFrame &ref, const Vector3 &p, const Vector3 &v, const Quaternion &q);

	/**
	 * Performs an IMU update.
	 *
	 * @param w  Angular velocity from the rate gyros in the body frame (in rad/s).
	 * @param a  Acceleration relative to free fall from the accelerometers in the body frame (in @f$ m / s^2 @f$).
	 * @param dt Time step in seconds.
	 */
	void update(const Vector3 &w, const Vector3 &a, const Fixed &dt);

	/**
	 * Performs a GPS correction.
	 *
	 * @param geo Position from the GPS device in geodetic coordinates.
	 * @param v   True ENU velocity from the GPS device in m/s.
	 */
	void correct(const GeoCoord &geo, const Vector3 &v);

	/**
	 * Gets the local navigation reference frame.
	 */
	const RefFrame& getRefFrame() const;

	/**
	 * Gets the current estimated position of the UAV in the navigation frame.
	 */
	const Vector3& getPosition() const;

	/**
	 * Gets the current estimated velocity of the UAV in the navigation frame.
	 */
	const Vector3& getVelocity() const;

	/**
	 * Gets the current orientation of the UAV in the navigation frame.
	 *
	 * This quaternion rotates vectors from the navigation frame to the body frame
	 * of the UAV.
	 */
	const Quaternion& getOrientation() const;

private:
	RefFrame refFrame;       ///< Local ENU navigation frame.
	Vector3 position;        ///< Position in navigation frame (in meters).
	Vector3 velocity;        ///< Velocity in navigation frame (in m/s).
	Quaternion orientation;  ///< Orientation quaternion (navigation to body frame).
	Vector3 gyroBias;        ///< Gyroscope bias estimates (in rad/s).
	Vector3 accelBias;       ///< Accelerometer bias estimates (in m/s^2).
	const Vector3 imuOffset; ///< IMU offset from center-of-gravity (in meters).
	const Vector3 gpsOffset; ///< GPS antenna offset from center-of-gravity (in meters).
};

inline Navigation::Navigation(const Vector3 &imuOffset, const Vector3 &gpsOffset)
	: imuOffset(imuOffset), gpsOffset(gpsOffset) {}

inline const RefFrame& Navigation::getRefFrame() const { return refFrame; }

inline const Vector3& Navigation::getPosition() const { return position; }

inline const Vector3& Navigation::getVelocity() const { return velocity; }

inline const Quaternion& Navigation::getOrientation() const { return orientation; }

}} // end namespace

#endif
